function SK6_Recieve_Data_Parse_Callback( port , event )
% SHAKE SK6 Matlab Driver v0.00, (c) 2010 SAMH Engineering Services
% Data parsing function (serial port timed call back function)

% total number of sensors that can be parsed
global num_sensors

% number of samples per sensor in the circular buffer
global BuffLength

% number of channels per sensor ( max is 5 for quaternions (4) plus count
% (1). The index of the count variable will always be 5
global BuffWidth

% the length of data packets ( 10 bytes comprises 3 header,6 data and one count byte )
global PacketLengths

% circular buffer that contains the parsed data
global BuffData

% the length of all headers ( with Sk6's raw data format )
persistent header_length

% headers is the list of possible data packet headers that are to be parsed
persistent headers

% the length of the longets packet
persistent max_packet_length

% data_fields is the list of the number of data fields within each packet type
global data_fields

% points to the position where new data is to be written
persistent BuffIn

% points to the position where data is to be read from
global BuffOut

% holds the number of items in the buffer
global BuffItems

% allocate a static buffer that is used to save un-parsed data until the next call to this function
persistent saved_buff;

% keep track of the number of bytes that were saved from last time
persistent saved_bytes;

% don't process the data for the first few hundred mili-seconds, just dump
% the data in the receive buffer
persistent startup_delay

if isempty( startup_delay )
  startup_delay = int32( 0 );
  % initialize variables just once
  BuffLength = int32( 1024 ); % number of samples per sensor in the circular buffer
  BuffWidth = int32( 5 ); % number of channels per sensor
  
  PacketLengths = int32( [ 10 10 10 6 6 6] ); % the length of data packets ( 10 bytes comprises 3 header,6 data and one count byte )
  headers = uint8(... % headers is the list of possible data packet headers that are to be parsed
    [...
    127 127 126 ;... % Acceleration header
    127 127 125 ;... % Gyro header
    127 127 124 ;... % Magnetometer header
    127 127 120 ;... % Analog input channel 0 header
    127 127 119 ;... % Analog input channel 1 header
    127 127 118 ]... % Navigation switch header
    );
  
  %data_fields = int32( [ 3 3 3 4 4 3 1 1 1 3 ] ); % data_fields is the list of the number of data fields within each packet type
  data_fields = int32( [ 3 3 3 1 1 1] ); % data_fields is the list of the number of data fields within each packet type
  size_headers = int32( size( headers) );
  num_sensors = size_headers( 1 ); % total number of sensors that can be parsed
  max_packet_length = int32( max ( PacketLengths ) ); % the length of the longets packet
  saved_buff = uint8( zeros ( 1 , max_packet_length ) ); % allocate a static buffer that is used to save un-parsed data until the next call to this function
  saved_bytes = int32( 0 ); % keep track of the number of bytes that were saved from last time
  BuffData = int32( zeros ( num_sensors , BuffLength ,  BuffWidth ) ); % circular buffer that contains the parsed data
  header_length = int32( 3 ); % the length of all headers ( with Sk6's raw data format )
  BuffIn =  int32( ones ( 1 , num_sensors ) ); % points to the position where new data is to be written
  BuffOut = int32( ones ( 1 , num_sensors ) ); % points to the position where data is to be read from
  BuffItems = int32( zeros ( 1 , num_sensors ) );% holds the number of items in the buffer
  
end
if startup_delay < 10
  startup_delay = startup_delay + 1;
  if port.BytesAvailable > 0
    fread( port , port.BytesAvailable , 'uint8' );
  end
  return
end



%+++++++++++++++++++++++++++++++++
% global variables, available to the main application
%+++++++++++++++++++++++++++++++++


% find the number of bytes available to read from the serial port input
% buffer
serial_bytes_available = int32( port.BytesAvailable );

% warn if the number of serial bytes available is high - an indication that
% the processor load is too high
if serial_bytes_available > 2000
  fprintf( 2 , 'Input buffer backlog, serial bytes available is greater than 2000\n' );
end

% add the number of bytes saved from last time to find the required buffer
% size
input_buff_size = int32( serial_bytes_available + saved_bytes );

% create the input buffer block
input_buff = uint8( zeros ( 1 , input_buff_size ) );

% add the saved bytes from last time to the start of the buffer
input_buff( 1 : saved_bytes ) = saved_buff( 1 : saved_bytes );

% if there are bytes available, read them all into the local buffer
if serial_bytes_available > 0
  input_buff( saved_bytes + 1 : input_buff_size )  = fread( port , double( serial_bytes_available ), 'uint8' );
else
  % there are no bytes available, exit
  return
end

% the index into the input data buffer, this always points to the first
% header byte of a packet
input_buff_index = int32( 1 );

% the number of bytes in the input buffer that remain to be parsed
bytes_left = int32( 0 );

% create the temporary matrix for holding the un-parsed raw data ( without the headers )
raw_matrix = uint8( zeros( num_sensors , ( ( input_buff_size * 3 ) / 2 ) ));

% create the temporary matrix for holding the counter fields
count_matrix = uint8( zeros( num_sensors , input_buff_size / 6 ));


% create a count array of the number of each sensors' packets that are to be parsed
packet_count = int32( zeros( 1 , num_sensors ) );


while true
  % check if there is at least enough bytes for one packet
  bytes_left = input_buff_size - input_buff_index + 1;
  if bytes_left < max_packet_length
    % not enough bytes in the buffer to parse the largest packet size,
    % save the remaining bytes until next time, convert the already parsed packets and exit
    
    for sensor = 1 : num_sensors
      if ( packet_count( sensor ) > 0 )
        % variable storing the number of data bytes in the header packet
        data_bytes = data_fields( sensor ) * 2;
        
        % calculate the number of spaces that are available between the
        % current input pointer and the end of the buffer
        spaces_at_end_of_buffer = BuffLength - BuffIn( sensor ) + 1;
        
        if ( packet_count( sensor ) <= spaces_at_end_of_buffer )
          % enough space in the output buffer without a wrap
          
          % add all the new packets to the output buffer
          BuffData( sensor , BuffIn( sensor ):BuffIn( sensor ) + packet_count( sensor ) - 1, 1:data_bytes / 2 ) = ...
            reshape( typecast( raw_matrix ( sensor , 1 : packet_count( sensor ) * data_bytes ) , 'int16') , data_bytes / 2 , [] )';
          
          % add all the count fields to the output buffer
          BuffData( sensor , BuffIn( sensor ):BuffIn( sensor ) + packet_count( sensor ) - 1, 5 ) = ...
            int32( count_matrix ( sensor , 1 : packet_count( sensor ) ) );
          
          % update the output buffer in pointer
          BuffIn( sensor ) = BuffIn( sensor ) + packet_count( sensor );
        else
          % not enough space in buffer without a wrap
          
          % transfer the first block ( the pre-wrap part )
          BuffData( sensor , BuffIn( sensor ) : BuffLength, 1:data_bytes / 2 ) = ...
            reshape( typecast( raw_matrix ( sensor , 1 : spaces_at_end_of_buffer * data_bytes ) , 'int16') , data_bytes / 2 , [] )';
          
          % add all the count fields to the output buffer
          BuffData( sensor , BuffIn( sensor ) : BuffLength, 5 ) = ...
            int32( count_matrix ( sensor , 1 : spaces_at_end_of_buffer ) );
          
          % calculate the number of spaces at the start of the buffer
          spaces_at_start_of_buffer = packet_count( sensor ) - spaces_at_end_of_buffer;
          
          % add the items to the start of the output buffer
          BuffData( sensor , 1 : spaces_at_start_of_buffer , 1:data_bytes / 2 ) = ...
            reshape( typecast( raw_matrix ( sensor , spaces_at_end_of_buffer * data_bytes + 1 : packet_count( sensor ) * data_bytes ) , 'int16') , data_bytes / 2 , [] )';
          
          % add all the count fields to the output buffer
          BuffData( sensor , 1:spaces_at_start_of_buffer , 5 ) = ...
            int32( count_matrix ( sensor , spaces_at_end_of_buffer + 1 : packet_count( sensor ) ) );
          
          % update the buffer input index
          BuffIn( sensor ) = spaces_at_start_of_buffer + 1;
        end
        % update the output buffer items count
        BuffItems( sensor ) = BuffItems( sensor ) + packet_count( sensor );
      end
    end
    
    % calculate and save the number of bytes that are to be saved
    saved_bytes = bytes_left;
    % save the bytes
    if saved_bytes > 0
      saved_buff( 1 : saved_bytes ) = input_buff( input_buff_index : input_buff_size );
    end
    return;
  else
    
    % check for a valid header
    for sensor = 1 : num_sensors
      if  all( headers( sensor , 1:header_length ) == input_buff( input_buff_index : input_buff_index + header_length - 1 ))
        % variable storing the number of data bytes in the header packet
        data_bytes = data_fields( sensor ) * 2;
        %header found, transfer data to the raw_matrix if there is room
        
        if packet_count( sensor )  < ( BuffLength - BuffItems( sensor ) )
          % extract the raw data
          raw_matrix( sensor , packet_count( sensor ) * data_bytes + 1 : ( packet_count( sensor ) + 1 ) * data_bytes ) = ...
            input_buff( input_buff_index + header_length : input_buff_index + header_length + data_bytes - 1 );
          % extract the counter field
          count_matrix( sensor , packet_count( sensor ) + 1 ) = ...
            input_buff( input_buff_index + header_length + data_bytes );
          
          % increase the count of the number of packets found
          packet_count( sensor ) = packet_count( sensor ) + 1;
        end
        
        % move index pointer to just after the parsed packet
        input_buff_index = input_buff_index + PacketLengths( sensor ) - 1;
        break; % move on through input buffer
      end
      
    end
    % move on to next byte in input buffer
    input_buff_index = input_buff_index + 1;
  end
  
end % while true


end %function
